import threading
import time

from src.connectors.modbus_connector import ModbusConnector
from src.logger.motor_logger import MotorLogger
from src.utils.decimal_conversion import DecimalConversion


class LinearMotorDriver:
    """
    linear motor driver
    """

    def __init__(self):
        # 日志
        self.logger = MotorLogger()
        self.modbus_connector = ModbusConnector.get_instance()
        self.modbus_connector.modbus_rtu_client()
        self.modbus_connector.open_client_device()

    # def forward_move_speed_mode(self):
    #     # 打开电机使能。参数编号F0601
    #     self.modbus_connector.rtu_write_single_register(1, 1537, 1)
    #
    #     # 设置速度值
    #     # self.modbus_connector.rtu_write_single_register(100,,1)
    #
    #     # 速度模式下开始移动
    #
    #     # 正向移动
    #
    #     # 反向移动
    #
    #     # 额定电压
    #     res = self.modbus_connector.rtu_read_holding_register(16, 1, 1)
    #     print(f"res is: {res}")
    #
    # def choose_control_model(self):
    #     """
    #     选择控制模式
    #     0-位置模式
    #     1-速度模式
    #     2-转矩模式
    #     :return:
    #     """
    #     # 打开电机使能。参数编号F0601
    #     self.modbus_connector.rtu_write_single_register(1, 1537, 1)
    #
    # def get_motor_type(self):
    #     """
    #     get motor type
    #     F0001，电机类型选择
    #     :return:
    #     """
    #     res = self.modbus_connector.rtu_read_holding_register(0, 1, 1)
    #     print(f"motor type res is: {res}")

    def get_motor_driver_status(self):
        res = self.modbus_connector.rtu_read_holding_register(1, 1, 1)
        self.logger.info_log(f"motor driver status res is: {res}")

    def get_driver_id(self):
        res = self.modbus_connector.rtu_read_holding_register(93, 1, 1)
        self.logger.info_log(f"driver id is: {res}")

    def get_driver_version(self):
        res = self.modbus_connector.rtu_read_holding_register(94, 1, 1)
        self.logger.info_log(f"driver version is: {res}")

    def get_driver_status(self) -> str:
        """
        读取驱动器状态标志寄存器，地址：1。获取驱动器状态，
        :return:
        """
        res = self.modbus_connector.rtu_read_holding_register(1, 1, 1)
        self.logger.info_log(f"driver status register value is: {res}")

        status_int = res[0]
        # status_bin = f'{status_int:016b}'
        status_bin = format(status_int, "016b")
        status_str = str(status_bin)
        self.logger.info_log(f"driver status binary value is: {status_str}")
        return status_str

    def get_driver_alarm(self):
        """
        读取驱动器报警标志寄存器，地址：0
        :return:
        """
        res = self.modbus_connector.rtu_read_holding_register(0, 1, 1)
        self.logger.info_log(f"driver alarm register value is: {res}")
        return res

    def set_continue_speed_rotate(self):
        """
        set motor continue speed rotate
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(3, 18, 1)
        self.logger.info_log(f"continue speed rotate is: {res}")

    def get_internal_pulse_mode(self):
        """
        read internal pulse mode
        register address: 20
        内部脉冲模式时预设应用程序选择
        0——响应 18 寄存器的指令
        1——回零模式
        2——预置 IO 控制模式一：起停+方向
        3——预置 IO 控制模式二：正转+反转
        4——预置 IO 控制模式三：内部速度表
        5——预置 IO 控制模式四：内部位置表
        :return:
        """
        res = self.modbus_connector.rtu_read_holding_register(20, 1, 1)
        self.logger.info_log(f"internal pulse mode is: {res}")

    def set_motor_run_mode(self, run_mode: int = 3):
        """
        set motor internal pulse mode
        设置运动模式，3-正转+反转
        内部脉冲模式时预设应用程序选择
        0——响应 18 寄存器的指令
        1——回零模式
        2——预置 IO 控制模式一：起停+方向
        3——预置 IO 控制模式二：正转+反转
        4——预置 IO 控制模式三：内部速度表
        5——预置 IO 控制模式四：内部位置表
        6：预置IO控制模式五：步进位置
        7：客户定制1
        8：客户定制2
        9：客户定制3
        10：客户定制4
        11：客户定制5
        12：客户定制6
        13：客户定制7
        14：客户定制8
        15：客户定制9
        16：客户定制10
        17：客户定制11
        18：客户定制12
        19：客户定制13
        20：客户定制14
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(run_mode, 20, 1)
        self.logger.info_log(f"set motor run mode result is: {res}")

    def set_pulse_count_one_circle(self, count: int):
        """
        设置电机细分（脉冲数/转）
        :param count:
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(count, 24, 1)
        self.logger.info_log(f"set pulse count one circle, result is:{res}")

    def get_pulse_count_one_circle(self) -> list:
        """
        获取电机细分（脉冲数/转）
        :return:
        """
        res = self.modbus_connector.rtu_read_holding_register(24, 1, 1)
        self.logger.info_log(f"one circle pulse count is: {res}")
        return res

    def fixed_length_start_motor(self, rotation_direction: int = 1):
        """
        定长运动旋转
        内部脉冲模式时的控制指令
        0——等待状态，驱动器接收到任何控制指
        令，经过驱动器处理以后后将恢复位等待状
        态。所以读取本寄存器总是返回 0。
        1——定长正转。
        在相对位置模式下，电机依据 70~74 寄存器
        参数正向运行。
        在绝对位置模式下，依据当前位置及 70~74
        设置的绝对位置决定运行状态。
        2——定长反转。
        在相对位置模式下，电机依据 70~74 寄存器
        参数反向运行。
        在绝对位置模式下，依据当前位置及 70~74
        设置的绝对位置决定运行状态。
        3——速度模式，连续正转。电机依据 75、76
        寄存器进行正向加速运行
        4——速度模式，连续反转。电机依据 75、76
        寄存器进行反向加速运行
        5——急停。电机依据 77 寄存器进行减速停
        止
        6——减速停止。
        位置模式，电机依据 71 寄存器进行减速停止
        速度模式，电机依据 76 寄存器进行减速停止
        :return:
        """
        fixed_length_start_motor = self.modbus_connector.rtu_write_single_register(rotation_direction, 18, 1)
        self.logger.info_log(f"fixed length start motor res is: {fixed_length_start_motor}")

    def start_motor(self, rotate_direction: int = 7):
        """
        set start motor
        输入端口功能选择
        0：脉冲输入
        1：方向输入
        2：正交编码器A相输入
        3：正交编码器B相输入
        4：电机脱机
        5：清除故障
        6：急停
        7：点动正转/启停
        8：点动反转/方向
        9：正向限位输入
        10：反向限位输入
        11：零点信号
        12：启动回零
        13：电机运行方向取反
        14：多段速度控制0
        15：多段速度控制1
        16：多段速度控制2
        17：多段速度控制3
        18：多段位置控制0
        19：多段位置控制1
        20：多段位置控制2
        21：多段位置控制3
        22：USER1
        23：USER2
        24：USER3
        25：USER4
        26：USER5
        27：USER6
        28：USER7
        29：USER8
        30：USER9
        31：USER10
        其他：输入口无作用，只做普通输入口
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(rotate_direction, 63, 1)
        self.logger.info_log(f"start motor is: {res}")

    def point_io_forward_rotate(self):
        """
        IO控制模式向前转
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(7, 63, 1)
        self.logger.info_log(f"io control point forward rotate res is: {res}")

    def point_io_back_rotate(self):
        """
        IO控制模式向后转
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(8, 63, 1)
        start_res = self.modbus_connector.rtu_write_single_register(7, 64, 1)
        self.logger.info_log(f"io control point back rotate res is: {res}, start run res is: {start_res}")

    def point_io_motor_stop(self):
        """
        IO控制模式停止
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(27, 63, 1)
        self.logger.info_log(f"io control point, stop motor res is: {res}")

    def point_io_emergency_stop(self):
        """
        IO控制模式，急停
        :return:
        """
        # 设置急停时的减速度
        emergency_stop_decelerate = self.modbus_connector.rtu_write_single_register(1000, 78, 1)
        self.logger.info_log(f"io control point, set emergency stop decelerate result is: {emergency_stop_decelerate}")
        res = self.modbus_connector.rtu_write_single_register(6, 63)
        self.logger.info_log(f"io control point, emergency stop res is: {res}")

    def emergency_stop(self):
        """
        急停
        :return:
        """
        # 设置急停时的减速度
        emergency_stop_decelerate = self.modbus_connector.rtu_write_single_register(1000, 78, 1)
        self.logger.info_log(f"set emergency stop decelerate result is: {emergency_stop_decelerate}")
        res = self.modbus_connector.rtu_write_single_register(6, 63, 1)
        # res = self.modbus_connector.rtu_write_single_register(5, 18, 1)
        self.logger.info_log(f"start motor is: {res}")

    def start_to_zero(self):
        """
        启动回零
        :return:
        """
        # 选择回零模式
        zero_res = self.modbus_connector.rtu_write_single_register(1, 20, 1)
        self.logger.info_log(f"choice run_to_zero mode, result is: {zero_res}")
        res = self.modbus_connector.rtu_write_single_register(12, 63, 1)
        self.logger.info_log(f"start to zero result is: {res}")

    def confirm_zero_position(self):
        """
        通讯触发以当前位置为原点
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(6, 287, 1)
        self.logger.info_log(f"communication trigger with the current position as the origin result is: {res}")

    def stop_motor(self):
        """
        stop motor
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(27, 63, 1)
        print(f"stop motor is: {res}")

    def get_single_circle_encoder_pulse_count_value(self) -> list:
        """
        读取
        内部脉冲模式时，当前绝对位置
        0x08		内部脉冲模式时，当前绝对位置低 16 位
        0x09		内部脉冲模式时，当前绝对位置高 16 位
        :return:
        """
        # res = self.modbus_connector.rtu_read_holding_register(29, 1, 1)
        res = self.modbus_connector.rtu_read_holding_register(8, 2, 1)
        self.logger.info_log(f"current absolute position pulse: {res}")
        return res

    def clear_motor_current_absolute_position(self):
        """
        清除当前位置
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(1, 85, 1)
        self.logger.info_log(f"clear current absolute position is: {res}")

    # def get_motor_current_speed(self):
    #     """
    #     电机当前指令速度
    #     :return:
    #     """
    #     res = self.modbus_connector.rtu_read_holding_register(10, 1, 1)
    #     # print(f"motor current command speed res is: {res}")
    #     return res[0]

    def set_fixed_length_motion(self, choice_number: int):
        """
        set fixed length motion
        选择运动模式，选择3：正传+反转
        code: 20
        内部脉冲模式时预设应用程序选择
        0：响应18寄存器的指令
        1：保留，请勿使用
        2：预置IO控制模式一：起停+方向
        3：预置IO控制模式二：正转+反转
        4：预置IO控制模式三：内部速度表
        5：预置IO控制模式四：内部位置表
        6：预置IO控制模式五：步进位置
        7：客户定制1
        8：客户定制2
        9：客户定制3
        10：客户定制4
        11：客户定制5
        12：客户定制6
        13：客户定制7
        14：客户定制8
        15：客户定制9
        16：客户定制10
        17：客户定制11
        18：客户定制12
        19：客户定制13
        20：客户定制14
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(choice_number, 20, 1)
        self.logger.info_log(f"fixed length motion is: {res}")

    def set_position_mode(self, position_mode: int = 1):
        """
        choice position mode
        位置运动方式选择：增量（相对）运动与绝对运动
        0——增量运动
        1——绝对运动
        :return:
        """

        res = self.modbus_connector.rtu_write_single_register(position_mode, 84, 1)
        self.logger.info_log(f"position mode, choice relative motion is: {res}")

    def set_trip_pulse_count(self, pulse_count: int = 4000):
        """
        set pulse count when motor is running
        设置运动的行程，范围：[-16777216,16777216]
        脉冲数，0x49 为低16位数据，0x4A 为高16位数据
        :return: 0-设置失败，1-设置成功
        """
        self.logger.info_log(f"set trip pulse count paramters: {pulse_count}")
        if pulse_count > 16777216 and pulse_count < -16777216:
            self.logger.error_log(f"set trip pulse number out of range: {pulse_count}")
            return 0

        pulse_count_binary = DecimalConversion.signed_int_to_binary(pulse_count)
        self.logger.info_log(f"pluse count binary number is: {pulse_count_binary}")

        # 截取高位二进制字符串
        high_bit_str = pulse_count_binary[:-16]
        high_bit_int = int(high_bit_str, 2)
        # 截取低位二进制字符串
        low_bit_str = pulse_count_binary[-16:]
        low_bit_int = int(low_bit_str, 2)

        # 分别写入高位（74）和低位（73）寄存器
        high_write_res = self.modbus_connector.rtu_write_single_register(high_bit_int, 74, 1)
        self.logger.info_log(f"pulse count number, high write result is: {high_write_res}")
        low_write_res = self.modbus_connector.rtu_write_single_register(low_bit_int, 73, 1)
        self.logger.info_log(f"pulse count number, low write result is: {low_write_res}")

    def set_max_rotation_speed(self, rotation_speed: int):
        """
        点位运动参数设置，设置最大旋转速度，单位： RPM
        [0,3000]
        :return:
        """
        # self.modbus_connector.rtu_write_single_register(rotation_speed, 72, 1)
        # 77 电动模式设置旋转速度可成功
        self.modbus_connector.rtu_write_single_register(rotation_speed, 77, 1)
        self.logger.info_log(f"set max rotation speed is: {rotation_speed}")

    def set_acceleration(self, circle_count: int):
        """
        设置电机加速度
        单位：r/s^2
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(circle_count, 70, 1)
        print(f"set acceleration is: {res}")

    def set_deceleration(self, circle_count: int):
        """
        设置电机减速度
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(circle_count, 71, 1)
        print(f"set deceleration is: {res}")

    def set_motor_rotate_direction(self, rotate_direction: int = 1):
        """
        forward
        设置电机旋转方向，定长正传、反转
        1-定长正转
        2-定长反转
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(rotate_direction, 18, 1)
        print(f"rotate run forward result is: {res}")

    def set_point_io_acceleration(self, point_acceleration: int):
        """
        点动模式，设置加速度
        范围：[10,1000]，单位：r/s^2
        :param point_acceleration:
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(point_acceleration, 75, 1)
        self.logger.info_log(f"io control point, set acceleration is: {res}")

    def set_point_io_deceleration(self, point_deceleration: int):
        """
        点动模式，设置减速度
        范围：[10,1000]，单位：r/s^2
        :param point_deceleration: 减速度值
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(point_deceleration, 76, 1)
        self.logger.info_log(f"io control point, set deceleration is: {res}")

    def set_point_io_rotate_speed(self, rotate_speed: int):
        """
        点动模式，设置速度
        范围：[0,3000]，单位：RPM
        :param rotate_speed:
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(rotate_speed, 77, 1)
        self.logger.info_log(f"io control point, set rotate speed is: {res}")

    def set_point_emergency_stop_deceleration(self, emergency_stop_deceleration: int = 1000):
        """
        设置急停减速度
        范围：[10,1000]，单位：r/s^2
        :param emergency_stop_deceleration: 急停减速度
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(emergency_stop_deceleration, 78, 1)
        self.logger.info_log(f"io control point, set emergency stop deceleration res is: {res}")

    # def point_io_emergency_stop(self):
    #     """
    #     点动模式急停
    #     :return:
    #     """
    #     res = self.modbus_connector.rtu_write_single_register(5, 18, 1)
    #     self.logger.info_log(f"io control point, emergency stop res is: {res}")

    def point_io_decelerate_stop(self):
        """
        点动模式，减速停止
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(6, 18, 1)
        self.logger.info_log(f"io control point, decelerate stop res is: {res}")


if __name__ == "__main__":
    linear_motor = LinearMotorDriver()
    # linear_motor.get_motor_driver_status()
    # linear_motor.get_motor_current_speed()
    # linear_motor.get_driver_id()
    # linear_motor.get_driver_version()

    # linear_motor.set_continue_speed_rotate()
    # # linear_motor.set_internal_pulse_mode()
    # linear_motor.get_internal_pulse_mode()
    # linear_motor.start_motor()

    # t1 = threading.Thread(target=linear_motor.get_trip)
    # t1.start()
    #
    # t2 = threading.Thread(target=linear_motor.continue_run)
    # t2.start()

    # while True:
    #     # time.sleep(1)
    #     res = linear_motor.get_single_circle_encoder_pulse_count_value()
    #     # if res[0] == 4000:
    #     #     linear_motor.set_motor_rotate_direction(2)

    # linear_motor.get_single_circle_encoder_pulse_count_value()
    # time.sleep(1)
    # # 清除当前位置
    # linear_motor.clear_motor_current_absolute_position()
    # time.sleep(1)
    # linear_motor.get_single_circle_encoder_pulse_count_value()

    # linear_motor.get_pulse_count_one_circle()

    status = linear_motor.get_driver_status()
    status_int = status[0]
    # status_bin = f'{status_int:016b}'
    status_bin = format(status_int, "016b")
    print(f"status = {status_bin}")
    status_str = str(status_bin)
    print(f"motor zero status is: {status_str[-5]}")
